import math

dot_prod = lambda a,b: a*b.transpose()
elem_prod = None

def inter_params(sequence, index):
    floor_index = int(math.floor(index))
    ceil_index = int(math.ceil(index))

    floor_val = sequence[floor_index]
    ceil_val = sequence[ceil_index]

    diff_val = ceil_val - floor_val

    return (diff_val,floor_val, ceil_val, floor_index)

def inter_formula(diff_val,floor_val, ceil_val, floor_index):
    factor = (0.5-0.5*math.cos(math.pi*(index-floor_index)))
    result = floor_val + diff_val * factor

    return result


def inter_value(sequence, index):
    return inter_formula(inter_params(sequence, index))

def set_inter_value(sequence, index, value):
    (old_diff,floor_val, ceil_val, floor_index) = inter_params(sequence, index)

    new_diff = (value - floor_val) / (0.5-0.5*math.cos(math.pi*(index-floor_index)))
    d_diff = new_diff - old_diff

    index_diff = index - floor_index
    sequence[floor_index] -= index_diff * d_diff
    sequence[floor_index+1] += (1-index_diff) * d_diff



def snap(pos):
    for i in xrange(len(pos)):
        pos[i] = math.floor(pos[i]) + sigmoid(pos[i] % 1)

class Field(object):
    def __init__(self, values, start_pos, weights):
        self.values = values
        self.pos = start_pos
        self.weights = weights

    def move(motor_out):
        self.pos += TO_VECTOR(elem_prod(weights, motor_out))
        # with wraparound
        for p in xrange(len(self.pos)):
            dimlength = len(self.get_value(self.pos[:p]) - 1
            if self.pos[p] > dimlength:
                self.pos[p] -= dimlength


        snap(self.pos)

    def get_value(pos = None):
        if pos is None:
            pos = self.pos

        result = values
        for p in pos:
            result = result[p]

        for r in xrange(len(result)):
            result

        return result


